#include <Servo.h>
#include <math.h>
#define PI 3.14159265
#include <AccelStepper.h>
#define dirPin 2
#define stepPin 3
Servo S1;
Servo S2;
Servo S3;
int S1max=180,S1min=23;//maksymalny i minimalny kąt 1 serva
int S2max=180,S2min=0;//maksymalny i minimalny kąt 2 serva 
int a=179,b=145,c=53,e=90;//wymiary w mm

int dlugosc=0, wysokosc=0, kacik=0;
bool z=false, v=false, d=false;
int f=0;
float przeciw;

void check(float,float,float);//funkcja liczy kąty jakie potrzebujem dla serva1 i serva2
AccelStepper stepper = AccelStepper(1, stepPin, dirPin);

void setup() {
  Serial.begin(9600);
  pinMode(A0,INPUT_PULLUP);
  S1.attach(9);//tuuuuuu
  S2.attach(10);//tuuuuuu
  S3.attach(11);
  S2.write(90);
  S1.write(100);
  delay(10);
  pinMode(12,OUTPUT);
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(13,OUTPUT); // zasilanie silnika
  stepper.setMaxSpeed(1000);
  pinMode(4,OUTPUT); //stały sygnał dla 1/4 kroku
  digitalWrite(4,HIGH);
  
}
 
void loop() {

  if(z==false && v==false && Serial.available())
{ 
  dlugosc=Serial.parseInt();
  
z=true;
v=true;

}
if(v==true && d==false && Serial.available())
{
  wysokosc=Serial.parseInt();
v=false;
d=true;

}
if(d==true && z==true && Serial.available())
{
 kacik=Serial.parseInt();

d=false;
z=true;
}
if(dlugosc!=0 && wysokosc!=0)
{
  if(digitalRead(A0)==0)
  {
    digitalWrite(12,HIGH);
    int i = S2.read();
    int j = S1.read();
  
    int nom=S3.read();
    float katServ[2];
  kroki(dlugosc,kacik);
  if(nom<80){
     for(nom;nom<=80;nom++)
        {
        S3.write(nom);
        delay(50);
        } //koniec fora 
        }
      check(przeciw, wysokosc, katServ);
    idzie(katServ[0],katServ[1],50);
     delay(500);
     if(nom>54){
     for(nom;nom>=54;nom--)
        {
        S3.write(nom);
        delay(50);
        } //koniec fora 
        }
        delay(1000);
         wraca(120,30,50);
    delay(1000);
while(stepper.currentPosition() != 0)
  {
    stepper.setSpeed(200);
    stepper.runSpeed();
  }
if(nom<80){
     for(nom;nom<=80;nom++)
        {
        S3.write(nom);
        delay(50);
        } //koniec fora 
        }
    digitalWrite(12,LOW);
    }//przycisk
}
   
 }
void idzie(float pierwszy, float drugi, int szybkosc)
{
int i = S2.read();
int j = S1.read();
if(i<drugi){
     for(i;i<=drugi;i++)
        {
        S2.write(i);
        delay(szybkosc);
        } //koniec fora 
        }
    if(i>drugi){
     for(i;i>=drugi;i--)
        {
        S2.write(i);
        delay(szybkosc);
        } //koniec fora 
        }
    if(j<pierwszy){
    for(j;j<=pierwszy;j++)
        {
        S1.write(j);
        delay(szybkosc);
        } //koniec fora
        }
    if(j>pierwszy){
    for(j;j>=pierwszy;j--)
        {
        S1.write(j);
        delay(szybkosc);
        } //koniec fora
        }


}//koniec idzie

void wraca(float pierwszy, float drugi, int szybkosc)
{
int i = S2.read();
int j = S1.read();

    if(j<pierwszy){
    for(j;j<=pierwszy;j++)
        {
        S1.write(j);
        delay(szybkosc);
        } //koniec fora
        }
    if(j>pierwszy){
    for(j;j>=pierwszy;j--)
        {
        S1.write(j);
        delay(szybkosc);
        } //koniec fora
        }

  if(i<drugi){
     for(i;i<=drugi;i++)
        {
        S2.write(i);
        delay(szybkosc);
        } //koniec fora 
        }
    if(i>drugi){
     for(i;i>=drugi;i--)
        {
        S2.write(i);
        delay(szybkosc);
        } //koniec fora 
        }


}//koniec wraca

void kroki(float x,float y) {
int kroki;
float kat;
float katy;
float male=23;
 digitalWrite(13,LOW); //włączenie zasilania na silniku (przeciwnie do wskazówek zegara)

 
   stepper.setCurrentPosition(0);
   
   przeciw=sqrt(x*x+y*y);

   if(x>0 && y<0){
        kat = atan(y/x + male/przeciw);
  katy=(kat*180)/PI;
        kroki = katy*800/360;
   }
    if(x<0 && y<0){
        kat = atan(y/x + male/przeciw);
  katy=(kat*180)/PI;
        kroki =200 + katy*800/360;
   }
   if(x>0 && y>0){
        kat = atan(y/x + male/przeciw);
  katy=(kat*180)/PI;
        kroki = -(katy*800/360);
   }
    if(x<0 && y>0){
        kat = atan(y/x + male/przeciw);
  katy=(kat*180)/PI;
        kroki = -(katy*800/360)-200;
   }
 while(stepper.currentPosition() != kroki)
  {
    if(kroki>=0)
    {
      stepper.setSpeed(200);
    }
    else
    {
      stepper.setSpeed(-200);
    }
    stepper.runSpeed();
  }
  
 digitalWrite(13,HIGH);//wyłączenie zasilania na silniku
  delay(1000);
}


void check(float odlx,float wysy,float katServ[])
{
    int i,j;
    float h,X,D,y1,y2,kat1,kat2,odl,roznica,alfa,beta,odwrot;
    roznica=100;
    

    for (i=S1min;i<=S1max;i++)
    {
        for (j=S2min; j<=S2max;j++)
        {   
          odwrot=180-j;
            alfa=abs(i-90);
            if(i>=90)
            {   if((j+alfa)<180)
                beta=abs((odwrot-90+alfa));
                else
                beta=odwrot-180+alfa;
                
            }
            else
            {   
                if(odwrot<=alfa)
                beta=abs(alfa-odwrot);
                else
                beta=abs((odwrot-90-alfa));
                
            }
          
       
      kat1= alfa/180*PI;//kąt w radianach
            kat2= beta/180*PI;
          
            if(((i>=90) && ((odwrot+alfa)>=180)) || ((i<90) && (odwrot<=alfa)))
              {
                D=cos(kat2)*c;
                y2=sin(kat2)*c;
                X=cos(kat1)*b;
                y1=sin(kat1)*b;
              }
            else
              {
                X=cos(kat1)*b;
                D=sin(kat2)*c;
                y1=sin(kat1)*b;
                y2=cos(kat2)*c;
              }

            if((i>=90) && ((odwrot+alfa)>=180))
            h=a+y1+y2-e;
            else if((i>=90) && ((odwrot+alfa)<180))
            h=a+y1-y2-e;
            else if((i<90) && (odwrot<=alfa))
            h=a-y1+y2-e;
            else if((i<90) && (odwrot>alfa))
            h=a-y1-y2-e;

            if(((odwrot>=(90-alfa)) && (i>=90)) || ((i<90) &&  (odwrot>=(90+alfa))))
            odl=X+D;
            else
            odl=X-D;

            
            if ((roznica>=(abs(odlx-odl)+abs(wysy-h))) && (wysy<=h))
            {
                roznica=(abs(odlx-odl)+abs(wysy-h));//wazne
                katServ[0]=i+10; 
                katServ[1]=j; 
               
            }

        }
    }

}//koniec check